////////////////////////////////////
// version 0.6 de Roby
////////////////////////////////////
#include <Servo.h>

// globales
unsigned long _rapport_turrel_last = 0;
unsigned long _rapport_body_last = 0;
unsigned long _rapport_sensors_last = 0;
unsigned long _setting_rapport_stepping = 2;
unsigned long _setting_rapport_turrel_interval = 125*_setting_rapport_stepping;
unsigned long _setting_rapport_body_interval = 250*_setting_rapport_stepping;
unsigned long _setting_rapport_sensors_interval = 125*_setting_rapport_stepping;
boolean _send_status = false;
boolean _send_invalid_cmd = false;

// motors
const int E1 = 5; //M1 Speed Control
const int E2 = 6; //M2 Speed Control
const int M1 = 4; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

// servos of turrel
Servo servo_turrel_low, servo_turrel_high;

// IF Range Switch
const int infra_front_left = 10;
const int infra_front_right = 9;
const int infra_back_left = 11;
const int infra_back_right = 8;

// IF Range sensors
const int infra_range_turrel = 2;

// US Range sensors
const int sonic_range_turrel = 3;

int ibound(int iMin, int iMax, int iVal)
{
  if(iVal<iMin) return iMin;
  else if(iVal>iMax) return iMax;
  return iVal;
}

float body_position_x=0.0f;
float body_position_y=0.0f;
float body_position_z=0.0f;
float body_orientation_x=0.0f;
float body_orientation_y=0.0f;
float body_orientation_z=-1.0f;

void _fake_rotate(float speed)
{
  float vx=cos(speed)*body_orientation_x-sin(speed)*body_orientation_z;
  float vz=sin(speed)*body_orientation_x+cos(speed)*body_orientation_z;
		
  body_orientation_x = vx;
  body_orientation_z = vz;
}
void _fake_move(float speed)
{
  body_position_x += body_orientation_x*speed;
  body_position_y += body_orientation_y*speed;
  body_position_z += body_orientation_z*speed;
}
int _fake_left=0, _fake_front=0;
float _fake_fact = 0.0f;
unsigned long _fake_time=0;
void _fake_movement(int front, int left, float fact)
{
  unsigned long _time=millis();
  if(_fake_time>0) // deja en movement
  {
    unsigned long slice = _time-_fake_time;
    if(_fake_left<0)
      _fake_rotate(-slice*_fake_fact);
    else if(_fake_left>0)
      _fake_rotate(slice*_fake_fact);    
    if(_fake_front<0)
      _fake_move(-slice*_fake_fact);
    else if(_fake_front>0)
      _fake_move(slice*_fake_fact);    
  }
  _fake_left = left;
  _fake_front = front;
  _fake_time = _time;
  _fake_fact = fact;  
}

///////////////
// moteur
void motor_stop(void)
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
  _fake_movement(0, 0, 0);
}
void motor_action(char a,char b, boolean a2, boolean b2)
{
  if(a2==HIGH && b2==HIGH)
    _fake_movement(1, 0, 10.0f/1.0f);
  else if(a2==LOW && b2==LOW)
    _fake_movement(-1, 0, 10.0f/1.0f);
  else if(a2==HIGH && b2==LOW)
    _fake_movement(0, 1, 1.0f/4.0f);
  else if(a2==LOW && b2==HIGH)
    _fake_movement(0, -1, 1.0f/4.0f);
  analogWrite (E1,a);
  digitalWrite(M1,a2?HIGH:LOW);
  analogWrite (E2,b);
  digitalWrite(M2,b2?HIGH:LOW);
}
void setup_motor(void)
{
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
}

//////////////
// turrel
const int turrel_low_min = 10, turrel_low_mid = 80, turrel_low_max = 170;
const int turrel_high_min = 30, turrel_high_mid = 80, turrel_high_max = 105;
int current_turrel_position_low = turrel_low_mid;
int current_turrel_position_high = turrel_high_mid;

void reset_turrel_position(int dlay=50)
{
  current_turrel_position_low = turrel_low_mid;
  current_turrel_position_high = turrel_high_mid;
  servo_turrel_low.write(turrel_low_mid); 
  servo_turrel_high.write(turrel_high_mid);
  delay(dlay);    
}
void set_turrel_position(int low, int high)
{
  int set_low = ibound(turrel_low_min, turrel_low_max, low);
  int set_high = ibound(turrel_high_min, turrel_high_max, high);

  // calcul delay from old position to new position
  int ml = abs(current_turrel_position_low-set_low);
  int mh = abs(current_turrel_position_high-set_high);
  long dlay = (ml+mh)*2.5f;

  servo_turrel_low.write(set_low); 
  servo_turrel_high.write(set_high);
  current_turrel_position_low = set_low;
  current_turrel_position_high = set_high;
  
  delay(dlay);
}
void acc_turrel_position(int low, int high)
{
  set_turrel_position(current_turrel_position_low + low, current_turrel_position_high + high);
}
void setup_turrel()
{
  servo_turrel_low.attach(12);
  servo_turrel_high.attach(13);
  reset_turrel_position(50);
}

//////////////
// IF Range Swiches
int setup_rangeswiches(){
  pinMode(infra_front_left, INPUT);
  pinMode(infra_front_right, INPUT);
  pinMode(infra_back_left, INPUT);
  pinMode(infra_back_right, INPUT);
}

//////////////
// IR Range Sensors
short get_range_sensor_infra(int infra, int count=5, int dlay=5)
{
  float fDistance = 0;

  for(int i=0; i<count; i++)
  {
    if(dlay>0) delay(dlay);
  
    float fScale = 5.0f/1024.0f;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    int iAnalogValue = analogRead(infra);
    float fVolts = iAnalogValue*fScale;

    // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
    fDistance += 65.0f*pow(fVolts, -1.10f); 
  }

  // retour en cm
  return (short)(fDistance/count);
}
short get_range_sensor_sonic(int sonic, int count=5, int dlay=5)
{
  float distance = 0;

  for(int i=0; i<count; i++)
  {
    if(dlay>0) delay(dlay);

    long duration=0;
    pinMode(sonic, OUTPUT);
    digitalWrite(sonic, LOW);
    delayMicroseconds(2);
    digitalWrite(sonic, HIGH);
    delayMicroseconds(5);
    digitalWrite(sonic, LOW);

    pinMode(sonic, INPUT);
    duration = pulseIn(sonic, HIGH);
    
    distance += (float)(duration / 29 / 2);
  }
  
  // retour en cm
  return (short)(distance/count);
}

const int turrel_ir_min = 35, turrel_ir_max = 140;
const int turrel_us_min = 3, turrel_us_max = 600;
short get_range_turrel(int count=5, int dlay=5, boolean bUseSonic=true)
{
  // #tsl+35+115+1+75$
  short dist = get_range_sensor_infra(infra_range_turrel, count, dlay);
  if(dist > turrel_ir_max && bUseSonic)
  {
    short dist2 =  get_range_sensor_sonic(sonic_range_turrel, count, dlay);
    if(dist2>turrel_ir_max)
    {
      dist = dist2;
    }
  }
  return dist;
}

//////////////
// scan function !
void scan_point(int iLow, int iHigh)
{
  int old_low = current_turrel_position_low, old_high = current_turrel_position_high;

  // bounding limit
  int iL = ibound(turrel_low_min, turrel_low_max, iLow);
  int iH = ibound(turrel_high_min, turrel_high_max, iHigh);

  send_start_trame();
  Serial.print("scan_point:");
  Serial.print("data=");
  // set position
  set_turrel_position(iL, iH);
  // get distance
  short iDist = get_range_turrel();
  // write it
  Serial.print(iL);
  Serial.print(" ");
  Serial.print(iH);
  Serial.print(" ");
  Serial.print(iDist);    
  send_end_trame();

  set_turrel_position(old_low, old_high);
}
void scan_line(int iLowBegin, int iLowEnd, int iLowStep, int iHigh)
{
  int old_low = current_turrel_position_low, old_high = current_turrel_position_high;

  // bounding limit
  int iLB = ibound(turrel_low_min, turrel_low_max, iLowBegin);
  int iLE = ibound(turrel_low_min, turrel_low_max, iLowEnd);
  int iH = ibound(turrel_high_min, turrel_high_max, iHigh);

  set_turrel_position(iLB, iH);

  send_start_trame();
  Serial.print("scan_line:");
  Serial.print("low_start="); Serial.print(iLB); Serial.print(",");
  Serial.print("low_end="); Serial.print(iLE); Serial.print(",");
  Serial.print("low_step="); Serial.print(iLowStep); Serial.print(",");
  Serial.print("high="); Serial.print(iH); Serial.print(",");
  Serial.print("data=");
  for(int iL=iLB; iL<iLE+1; iL+=iLowStep)
  {
    // set position
    set_turrel_position(iL, iH);
    // get distance
    short iDist = get_range_turrel();

    // write it
    if(iL!=iLB)
      Serial.print(";");
    Serial.print(iL);
    Serial.print(" ");
    Serial.print(iH);
    Serial.print(" ");
    Serial.print(iDist);
  }
  send_end_trame();  
  set_turrel_position(old_low, old_high);
}
// do a line and return to start fow new one
void scan_zone_z(int iLowBegin, int iLowEnd, int iLowStep, int iHighBegin, int iHighEnd, int iHighStep)
{
  int old_low = current_turrel_position_low, old_high = current_turrel_position_high;

  // bounding limit
  int iLB = ibound(turrel_low_min, turrel_low_max, iLowBegin);
  int iLE = ibound(turrel_low_min, turrel_low_max, iLowEnd);
  int iHB = ibound(turrel_high_min, turrel_high_max, iHighBegin);
  int iHE = ibound(turrel_high_min, turrel_high_max, iHighEnd);
  
  set_turrel_position(iLB, iHB);

  send_start_trame();
  Serial.print("scan_zone:");
  Serial.print("low_start="); Serial.print(iLB); Serial.print(",");
  Serial.print("low_end="); Serial.print(iLE); Serial.print(",");
  Serial.print("low_step="); Serial.print(iLowStep); Serial.print(",");
  Serial.print("high_start="); Serial.print(iHB); Serial.print(",");
  Serial.print("high_end="); Serial.print(iHE); Serial.print(",");
  Serial.print("high_step="); Serial.print(iHighStep); Serial.print(",");
  Serial.print("data=");

  for(int iH=iHB; iH<iHE+1; iH+=iHighStep)
  {
    for(int iL=iLB; iL<iLE+1; iL+=iLowStep)
    {
      // set position
      set_turrel_position(iL, iH);
      
      // get distance
      short iDist = get_range_turrel();

      // write it
      if(!(iH==iHB && iL==iLB))
        Serial.print(";");
      Serial.print(iL);
      Serial.print(" ");
      Serial.print(iH);
      Serial.print(" ");
      Serial.print(iDist);
    }
  }
  send_end_trame();  
  set_turrel_position(old_low, old_high);
}
// do interval pair/unpair... quicker but bug some where :(
void scan_zone_s(int iLowBegin, int iLowEnd, int iLowStep, int iHighBegin, int iHighEnd, int iHighStep)
{
  int old_low = current_turrel_position_low, old_high = current_turrel_position_high;

  // bounding limit
  int iLB = ibound(turrel_low_min, turrel_low_max, iLowBegin);
  int iLE = ibound(turrel_low_min, turrel_low_max, iLowEnd);
  int iHB = ibound(turrel_high_min, turrel_high_max, iHighBegin);
  int iHE = ibound(turrel_high_min, turrel_high_max, iHighEnd);
  
  set_turrel_position(iLB, iHB);

  send_start_trame();
  Serial.print("scan_zone:");
  Serial.print("low_start="); Serial.print(iLB); Serial.print(",");
  Serial.print("low_end="); Serial.print(iLE); Serial.print(",");
  Serial.print("low_step="); Serial.print(iLowStep); Serial.print(",");
  Serial.print("high_start="); Serial.print(iHB); Serial.print(",");
  Serial.print("high_end="); Serial.print(iHE); Serial.print(",");
  Serial.print("high_step="); Serial.print(iHighStep); Serial.print(",");
  Serial.print("data=");

  boolean leftoright = true;
  for(int iH=iHB; iH<iHE+1; iH+=iHighStep)
  {
    int lb, le, ls;
    
    if (leftoright)
    {
      lb = iLB;
      le = iLE+1;
      ls = iLowStep;
    }
    else
    {
      lb = iLE;
      le = iLB-1;
      ls = -iLowStep;
    }
    leftoright = !leftoright;
    
    for(int iL=lb; (!leftoright && iL<le) || (leftoright && iL>le); iL+=ls)
    {
      // set position
      set_turrel_position(iL, iH);
      
      // get distance
      short iDist = get_range_turrel();

      // write it
      if(!(iH==iHB && iL==iLB))
        Serial.print(";");
      Serial.print(iL);
      Serial.print(" ");
      Serial.print(iH);
      Serial.print(" ");
      Serial.print(iDist);
    }
  }
  send_end_trame();  
  set_turrel_position(old_low, old_high);
}

//////////////
// main
void setup()
{
  Serial.begin(9600); // Set Baud Rate
  Serial.flush();
  setup_motor();
  setup_rangeswiches();
  setup_turrel();
}

//////////////////////////////
// trame
void send_start_trame()
{
  Serial.print("#");
}
void send_end_trame()
{
  Serial.println("$");
}

//////////////////////////////
// version
void send_version()
{
  send_start_trame();
  Serial.write("version:name=roby_bt,version=0.6");
  send_end_trame();
}

//////////////////////////////
// setting
void send_setting()
{
  send_start_trame();
  Serial.print("setting:");
  Serial.print("send_status=");
  Serial.print(_send_status?"1":"0");
  send_end_trame();  
}

void send_setting_body()
{
  send_start_trame();
  Serial.print("setting_body:");
  Serial.print("min_speed=0");
  Serial.print(",nice_speed=25");
  Serial.print(",max_speed=100");
  send_end_trame();
}

void send_setting_turrel()
{
  send_start_trame();  
  Serial.print("setting_turrel:");
  Serial.print("min_low=");
  Serial.print(turrel_low_min);
  Serial.print(",mid_low=");
  Serial.print(turrel_low_mid);
  Serial.print(",max_low=");
  Serial.print(turrel_low_max);
  Serial.print(",min_high=");
  Serial.print(turrel_high_min);
  Serial.print(",mid_high=");
  Serial.print(turrel_high_mid);
  Serial.print(",max_high=");
  Serial.print(turrel_high_max);
  Serial.print(",min_dist=");
  Serial.print(turrel_ir_min);
  Serial.print(",med_dist=");
  Serial.print(turrel_ir_max);
  Serial.print(",max_dist=");
  Serial.print(turrel_us_max);
  send_end_trame();
}

//////////////////////////////
// status
void send_status_sensors()
{
  boolean fl = (digitalRead(infra_front_left)==HIGH)?true:false;
  boolean fr = (digitalRead(infra_front_right)==HIGH)?true:false;
  boolean bl = (digitalRead(infra_back_left)==HIGH)?true:false;
  boolean br = (digitalRead(infra_back_right)==HIGH)?true:false;

  send_start_trame();
  Serial.print("sensors:");
  Serial.print("fl=");
  Serial.print(fl?"0":"1"); 
  Serial.print(",");
  Serial.print("fr="); 
  Serial.print(fr?"0":"1"); 
  Serial.print(",");
  Serial.print("bl="); 
  Serial.print(bl?"0":"1"); 
  Serial.print(",");
  Serial.print("br="); 
  Serial.print(br?"0":"1"); 
  send_end_trame();
}
void send_status_turrel()
{
  short dist = get_range_turrel();

  send_start_trame();
  Serial.print("turrel:");
  Serial.print("low=");
  Serial.print(current_turrel_position_low); 
  Serial.print(",");
  Serial.print("high="); 
  Serial.print(current_turrel_position_high); 
  Serial.print(",");
  Serial.print("dist="); 
  Serial.print(dist);
  send_end_trame();
}
void send_status_body()
{
  send_start_trame();
  Serial.print("body:");
  Serial.print("pos_x=");
  Serial.print(body_position_x); 
  Serial.print(",");
  Serial.print("pos_y="); 
  Serial.print(body_position_y); 
  Serial.print(",");
  Serial.print("pos_z="); 
  Serial.print(body_position_z);
  Serial.print(",");    
  Serial.print("ori_x=");
  Serial.print(body_orientation_x); 
  Serial.print(",");
  Serial.print("ori_y="); 
  Serial.print(body_orientation_y); 
  Serial.print(",");
  Serial.print("ori_z="); 
  Serial.print(body_orientation_z);
  send_end_trame();
}

//////////////////////////////
// serial buffer loading
const int _iMaxSerialBuffer = 512;
int _iSerialBufferStart=0, _iSerialBufferLen=0, _iSerialBufferIndex=0;
char _SerialBuffer[_iMaxSerialBuffer];
boolean readSerialString (String &cmd) {
  int sb;
  while (Serial.available())
  {
    sb = Serial.read();
    if(sb=='#') // start cmd ?
    {
      _iSerialBufferStart = _iSerialBufferIndex+1;
    }
    else if(sb=='$' && _iSerialBufferStart>=0) // end cmd ?
    {
      _iSerialBufferLen = _iSerialBufferIndex;
      _SerialBuffer[_iSerialBufferLen] = '\0';
      cmd = &_SerialBuffer[_iSerialBufferStart];
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return true;
    }
    if(_iSerialBufferIndex>=_iMaxSerialBuffer)
    {
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return false;
    }
    _SerialBuffer[_iSerialBufferIndex] = sb;
    _iSerialBufferIndex++;
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

void check_commands()
{
  String line="";
  if(!readSerialString(line))
    return;
  if(line.length()>0)
  {
    // debug only : Serial.println("cmd : "+line);

    switch(line[0])
    {
    case 'v': // (V)ersion
      {
        // #v$
        send_version();
      }
      break;      
    case 'g': // settin(G)
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 'b':  // (B)ody setting
              // #gab$
              send_setting_body();        
              break;
            case 't':  // (T)urrel setting
              // #gat$
              send_setting_turrel();
              break;
            default: // general setting
              // #ga-$
              send_setting();
              break;
            }
          }
          break;
        }
      } 
      break;            
    case 's': // (S)tatus    
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 's':  // (S)ensors
              // #sas$
              send_status_sensors();        
              break;
            case 't':  // (T)urrel
              // #sat$
              send_status_turrel();
              break;
            case 'b':  // (B)ody
              // #sab$
              send_status_body();
              break;
            }
          }
          break;
        case 's': // (S)et
          {
            switch(line[2])
            {
            case 's': // (S)end status automatic
              {
                // #sss0$/#sss1$
                char param = line[3];
                if(param=='0') _send_status = false;
                else if(param=='1') _send_status = true;
                send_setting();
              }
              break;
            }
          }
          break;
        }
      }
      break;
    case 'r': // (R)eset
      {
        switch(line[1])
        {
        case 'b': // (B)ody
          // #rb$
          motor_stop();
          break;
        case 't': // (T) turrel
          // #rt$
          reset_turrel_position();
          if(!_send_status) send_status_turrel();
          break;
        default: // All !
          // #r-$
          motor_stop(); 
          reset_turrel_position();
          if(!_send_status) send_status_turrel();
          break;
        }
      }
      break;      
    case 'b': // (B)ody stuff
      {
        switch(line[1])
        {
        case 'm': // 'M'oving
          {
            // #bm11+25+25$
            boolean bL = (line[2]=='0')?false:true;
            boolean bR = (line[3]=='0')?false:true;
            int iL=0, iR=0;
            int endl = load_int(line, 4, iL);
            load_int(line, endl, iR);
            int iPwnL = map(iL, 0, 100, 50, 250);
            int iPwnR = map(iR, 0, 100, 50, 250);
            motor_action(iPwnL, iPwnR, bL, bR);
          }
        }
      }
      break;      
    case 't': // (T)urrel stuff
      {
        switch(line[1])
        {
        case 'p': // set 'P'osition
          {
            // #tp+90+90$
            int iLow=0, iHigh=0;
            int endl = load_int(line, 2, iLow);
            load_int(line, endl, iHigh);
            set_turrel_position(iLow, iHigh);
            if(!_send_status) send_status_turrel();
          }
          break;
        case 's': // 'S'can mode !
          {
            switch(line[2])
            {
            case 'p':
              {
                // #tsp+90+90$
                int iLow=0, iHigh=0;
                int endl = load_int(line, 3, iLow);
                load_int(line, endl, iHigh);
                scan_point(iLow, iHigh);
                if(!_send_status) send_status_turrel();
              }
              break;
            case 'l':
              {
                // #tsl+85+95+1+75$
                int iLowBegin=0, iLowEnd=0, iLowStep=0, iHigh=0;
                int endl = load_int(line, 3, iLowBegin);
                endl = load_int(line, endl, iLowEnd);
                endl = load_int(line, endl, iLowStep);                
                load_int(line, endl, iHigh);
                scan_line(iLowBegin, iLowEnd, iLowStep, iHigh);
                if(!_send_status) send_status_turrel();
              }
              break;
            case 'z':
              {
                // #tsz+85+95+1+85+95+1$
                int iLowBegin=0, iLowEnd=0, iLowStep=0, iHighBegin=0, iHighEnd=0, iHighStep;
                int endl = load_int(line, 3, iLowBegin);
                endl = load_int(line, endl, iLowEnd);
                endl = load_int(line, endl, iLowStep);                
                endl = load_int(line, endl, iHighBegin);
                endl = load_int(line, endl, iHighEnd);                
                load_int(line, endl, iHighStep);
                scan_zone_z(iLowBegin, iLowEnd, iLowStep, iHighBegin, iHighEnd, iHighStep);
                if(!_send_status) send_status_turrel();
              }
              break;
            }
          }
          break;
        }
      }
      break;      
    default:
      if(_send_invalid_cmd)
      {
        send_start_trame();
        Serial.print("error:invalid cmd '");
        Serial.print(line+"'");
        send_end_trame();
      }
      break;
    }
  }
}

void check_hitsensors()
{
  if(  (digitalRead(infra_front_left)==HIGH)
    || (digitalRead(infra_front_right)==HIGH)
    || (digitalRead(infra_back_left)==HIGH)
    || (digitalRead(infra_back_right)==HIGH))
  {
    send_status_sensors();    
  }
}

///////////////////////////////
// loop
void loop(void)
{
  check_commands();
//  check_hitsensors();

  if(_send_status)
  {
    unsigned long _rapport_last = millis();
    if(( _rapport_last - _rapport_turrel_last) > _setting_rapport_turrel_interval)
    {
      send_status_turrel();
      _rapport_turrel_last =  _rapport_last;
    }
    if(( _rapport_last - _rapport_body_last) > _setting_rapport_body_interval)
    {
      send_status_body();
      _rapport_body_last =  _rapport_last;
    }
    if(( _rapport_last - _rapport_sensors_last) > _setting_rapport_sensors_interval)
    {
      send_status_sensors();
      _rapport_sensors_last =  _rapport_last;
    }
  }
}

























